#include "check.h"
// CHECK CLASS IMPLEMENTATION
Check::Check(){}

Check::Check(QString componentName_, CheckType type_, float thresholdValue_)
{
    componentName = componentName_;
    type = type_;
    thresholdValue = thresholdValue_;
    transformation = Eigen::Matrix4f::Identity();
}


QString Check::getComponentName()
{
    return componentName;
}

CheckType Check::getType()
{
    return type;
}

float Check::getThresholdValue()
{
    return thresholdValue;
}

Eigen::Matrix4f Check::getTransformation(Component* sourceComponent, Component* targetComponent, PointCloud<PointXYZRGB>::Ptr testCloud)
{
    if(transformation == Eigen::Matrix4f::Identity())
    {
        PointCloud<PointXYZRGB>::Ptr sourceComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr targetComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr registerdComponentCloud (new PointCloud<PointXYZRGB>);
        copyPointCloud(*sourceComponent->getCloud(), *sourceComponent->getIndices(), *sourceComponentCloud);
        copyPointCloud(*targetComponent->getCloud(), *targetComponent->getIndices(), *targetComponentCloud);
        // Given that the main clouds are already registered we can just perform the final registration on the clouds of our components to find the transformation.
        IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registrator (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
        registrator->setInputCloud(sourceComponentCloud);
        registrator->setInputTarget (targetComponentCloud);
        registrator->setMaxCorrespondenceDistance(4); // nuovo da tunare
        registrator->setRANSACOutlierRejectionThreshold (10);
        registrator->setRANSACIterations(100);
        registrator->setMaximumIterations (100); // 1st stop criterion
        registrator->setTransformationEpsilon (1e-9); // 2nd stop criterion (minumum difference between actual and previous transformation to continue)
        //registrator->setEuclideanFitnessEpsilon(1); // 3rd stop criterion (maximum allowed Euclidean error between two consecutive steps in the ICP loop)
        registrator->align(*testCloud);
        transformation = registrator->getFinalTransformation();
    }
    return transformation;
}

Eigen::Matrix4f Check::getTransformation(Component* sourceComponent, Component* targetComponent)
{
    time_t start,end;
    time (&start);
    if(transformation == Eigen::Matrix4f::Identity())
    {
        PointCloud<PointXYZRGB>::Ptr sourceComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr targetComponentCloud (new PointCloud<PointXYZRGB>);
        PointCloud<PointXYZRGB>::Ptr registerdComponentCloud (new PointCloud<PointXYZRGB>);
        copyPointCloud(*sourceComponent->getCloud(), *sourceComponent->getIndices(), *sourceComponentCloud);
        copyPointCloud(*targetComponent->getCloud(), *targetComponent->getIndices(), *targetComponentCloud);
        // Given that the main clouds are already registered we can just perform the final registration on the clouds of our components to find the transformation.
        IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registrator (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
        registrator->setInputCloud(sourceComponentCloud);
        registrator->setInputTarget (targetComponentCloud);
        registrator->setMaxCorrespondenceDistance(4); // nuovo da tunare
        registrator->setRANSACOutlierRejectionThreshold (10);
        registrator->setRANSACIterations(100);
        registrator->setMaximumIterations (100); // 1st stop criterion
        registrator->setTransformationEpsilon (1e-9); // 2nd stop criterion (minumum difference between actual and previous transformation to continue)
        //registrator->setEuclideanFitnessEpsilon(1); // 3rd stop criterion (maximum allowed Euclidean error between two consecutive steps in the ICP loop)
        registrator->align(*registerdComponentCloud);
        transformation = registrator->getFinalTransformation();
    }
    time (&end);
    double dif = difftime (end,start);
    cout << "------------------------------------->  Get component transformation in " << dif << " seconds"<< endl;
    return transformation;
}

bool Check::presenceCorrect(Component source, Component target, float threshold)
{
    int minimumNumberOfPoints = target.getIndices()->indices.size() * (threshold)/100;
    cout << "PRESENCE[num of points]: Source="<< source.getIndices()->indices.size()<<
           " - Target=" << target.getIndices()->indices.size()<<
           " - Threshold="<< threshold << " - Minimum=" << minimumNumberOfPoints;
    if (source.getIndices()->indices.size() >= minimumNumberOfPoints)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool Check::positionCorrect(Component source, Component target, float threshold)
{
    PointXYZRGB sourceComponentCentroid = source.getCenterOfMass();
    PointXYZRGB targetComponentCentroid = target.getCenterOfMass();
    float centroidDistance = euclideanDistance(sourceComponentCentroid,targetComponentCentroid);
    cout<< "POSITION[mm]: Difference = "<< centroidDistance << " - Threshold = " << threshold;
    if (centroidDistance < threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool Check::inclinationCorrect(Eigen::Matrix4f transformation, float threshold)
{
    Eigen::Matrix3f rotationMatrix = transformation.topLeftCorner(3,3);
    Eigen::Vector3f eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
    Eigen::Matrix3f inclinationMatrix;
    inclinationMatrix = Eigen::AngleAxisf(eulerAngles[0], Eigen::Vector3f::UnitX()) *
            Eigen::AngleAxisf(eulerAngles[1],  Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf angleAxis(inclinationMatrix);
    float rotation2 = rad2deg(angleAxis.angle());
    cout << "INCLINATION[°]: Angle = " << rotation2 << " - Threshold = " << threshold ;
    if (rotation2 < threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}

bool Check::orientationCorrect(Eigen::Matrix4f transformation, float threshold)
{
    Eigen::Matrix3f rotationMatrix;
    rotationMatrix = transformation.topLeftCorner(3,3);
    Eigen::Vector3f eulerAngles = rotationMatrix.eulerAngles(0, 1, 2); //extract roll, pitch, yaw
    cout << "ORIENTATION[°]: Angle = " << rad2deg(eulerAngles[2]) << " - Threshold = " << threshold;
    if(abs(rad2deg(eulerAngles[2])) < threshold)
    {
        cout << " ==> OK!" <<endl;
        return true;
    }
    else
    {
        cout << " ==> ERROR!" <<endl;
        return false;
    }
}
